/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "World.h"
#include "Sick.h"
#include "owndebug.h"

#define NUMBER_OF_RAYS 181
#define MIN_ANGLE (-M_PI/2.0)
#define MAX_ANGLE (M_PI/2.0)
#define RANGE 80.0


CSick::CSick(CWorld *World, CObject *ParentObject, dBodyID parentBody, 
          dReal *posOffset, dMatrix3 rotOffset, float interval, 
          MaCI::MaCICtrl::CMaCICtrlServer *mcs, int minor, std::string interfaceName)
:CLaserScanner(World,ParentObject,parentBody,posOffset, rotOffset, interval, 
    NUMBER_OF_RAYS, RANGE, MIN_ANGLE, MAX_ANGLE, mcs, minor, interfaceName),

PosOffset(posOffset),
material(DEFAULT_MATERIAL),
box(0.15,0.07,0.15,this,this->SpaceID,parentBody,&material,false),
box2(0.15,0.1,0.05,this,this->SpaceID,parentBody,&material,false)
{
  for (int i=0; i<12;i++)
      this->RotOffset[i]=rotOffset[i];
  
  
  float o1[3]={0, 0.055, 0};
  float o2[3]={0, -0.03, -0.05};
  float p1[3];
  float p2[3];
  
  RotateVector(RotOffset,o1,p1);
  RotateVector(RotOffset,o2,p2);
  
  dGeomSetOffsetPosition(box.GeomID, posOffset[0]+p1[0], posOffset[1]+p1[1], posOffset[2]+p1[2]);
  dGeomSetOffsetRotation(box.GeomID, rotOffset);

  dGeomSetOffsetPosition(box2.GeomID, posOffset[0]+p2[0], posOffset[1]+p2[1], posOffset[2]+p2[2]);
  dGeomSetOffsetRotation(box2.GeomID, rotOffset);
  
//   dGeomSetOffsetPosition(box.GeomID,posOffset[0],posOffset[1],posOffset[2]+0.055);
//   dGeomSetOffsetRotation(box.GeomID,rotOffset);
// 
//   dGeomSetOffsetPosition(box2.GeomID,posOffset[0]-0.05,posOffset[1],posOffset[2]-0.03);
//   dGeomSetOffsetRotation(box2.GeomID,rotOffset);
  
  material.look.color[0]=0.7;
  material.look.color[1]=0.7;
  material.look.color[2]=0.55;
  
  //rayCluster->IsVisible=true;
}
    
void CSick::Draw()
{
  dReal M[16];
  box.Draw();
  box2.Draw();
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    box.GetGeomTransMatrix(M);
    glMultMatrixf(M);

    glColor3f(material.look.color[0],material.look.color[1],material.look.color[2]);
    float mcolor[3] = {material.look.color[0], material.look.color[1], material.look.color[2]}; 
    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);




    glTranslatef(0,-0.085,-0.05);
    glTranslatef(0,-0.055,0.05);
    drawBox(0.15,0.01,0.15);
    
    glColor3f(0.2,0,0);
    mcolor[0] = 0.2;
    mcolor[1] = 0;
    mcolor[2] = 0;
    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);


    glTranslatef(0,0.055,0);
    glRotatef(90,1,0,0);
    drawCylinder(0.07,0.1,PRIMITIVE_DETAIL_LEVEL);

  glPopMatrix();
  glPopAttrib();
  
  rayCluster->Draw();
}
